#include "imu_gps_localizer/imu_processor.h"

#include <glog/logging.h>
#include <Eigen/Dense>

#include "imu_gps_localizer/utils.h"

namespace ImuGpsLocalization
{

    ImuProcessor::ImuProcessor(const double acc_noise, const double gyro_noise,
                               const double acc_bias_noise, const double gyro_bias_noise,
                               const Eigen::Vector3d &gravity)
        : acc_noise_(acc_noise), gyro_noise_(gyro_noise),
          acc_bias_noise_(acc_bias_noise), gyro_bias_noise_(gyro_bias_noise),
          gravity_(gravity) {}

    void ImuProcessor::Predict(const ImuDataPtr last_imu, const ImuDataPtr cur_imu, State *state)
    {
        // Time.
        const double delta_t = cur_imu->timestamp - last_imu->timestamp;
        const double delta_t2 = delta_t * delta_t;

        // Set last state.
        State last_state = *state;

        // Acc and gyro  // 去除 bias   // 初始化偏差为 0, 然后通过 kf 更新这两个偏差
        const Eigen::Vector3d acc_unbias = 0.5 * (last_imu->acc + cur_imu->acc) - last_state.acc_bias;
        const Eigen::Vector3d gyro_unbias = 0.5 * (last_imu->gyro + cur_imu->gyro) - last_state.gyro_bias;

        // Normal state.
        // Using P58. of "Quaternion kinematics for the error-state Kalman Filter".
        // https://zhuanlan.zhihu.com/p/152662055
        // 位置： p = p + v* delta_t + 0.5*(R*(a_m - a_b)) + g ) * delta_t^2
        state->G_p_I = last_state.G_p_I + last_state.G_v_I * delta_t +
                       0.5 * (last_state.G_R_I * acc_unbias + gravity_) * delta_t2;
        // 速度： v= v + (R*(a_m-a_b)+g)*delta_t
        state->G_v_I = last_state.G_v_I + (last_state.G_R_I * acc_unbias + gravity_) * delta_t;

        const Eigen::Vector3d delta_angle_axis = gyro_unbias * delta_t;
        if (delta_angle_axis.norm() > 1e-12) // 角度增量
        {
            // 旋转矩阵
            state->G_R_I = last_state.G_R_I * Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized()).toRotationMatrix();
        }
        // Error-state. Not needed.

        // https://zhuanlan.zhihu.com/p/88756311
        // ESKF预测部分的协方差传播方程为：P = F_x * P* F_x^T + F_i*Q_i*F_i^T
        // Covariance of the error-state  // 状态转移矩阵 F_x 矩阵
        Eigen::Matrix<double, 15, 15> Fx = Eigen::Matrix<double, 15, 15>::Identity();
        Fx.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity() * delta_t;
        Fx.block<3, 3>(3, 6) = -state->G_R_I * GetSkewMatrix(acc_unbias) * delta_t;
        Fx.block<3, 3>(3, 9) = -state->G_R_I * delta_t;
        if (delta_angle_axis.norm() > 1e-12)
        {
            Fx.block<3, 3>(6, 6) = Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized()).toRotationMatrix().transpose();
        }
        else
        {
            Fx.block<3, 3>(6, 6).setIdentity();
        }
        Fx.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity() * delta_t;

        // 系统输入雅可比矩阵
        Eigen::Matrix<double, 15, 12> Fi = Eigen::Matrix<double, 15, 12>::Zero();
        Fi.block<12, 12>(3, 0) = Eigen::Matrix<double, 12, 12>::Identity();

        // 过程噪声矩阵  // 状态转移协方差矩阵
        Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();
        Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();
        Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();
        Qi.block<3, 3>(6, 6) = delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();
        Qi.block<3, 3>(9, 9) = delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();

        // 协方差预测
        state->cov = Fx * last_state.cov * Fx.transpose() + Fi * Qi * Fi.transpose();

        // Time and imu.
        state->timestamp = cur_imu->timestamp;
        state->imu_data_ptr = cur_imu;
    }

} // namespace ImuGpsLocalization